import time
import sim
import math
def connect_to_coppeliasim(port=19999):
    sim.simxFinish(-1)  # 关闭所有可能存在的连接
    clientID = sim.simxStart('127.0.0.1', port, True, True, 5000, 5)
    if clientID != -1:
        print('Connected to CoppeliaSim')
    else:
        raise Exception('Failed to connect to CoppeliaSim')
    return clientID

def get_joint_angle(clientID, joint_name):
    # 获取关节句柄
    _, jointHandle = sim.simxGetObjectHandle(clientID, joint_name, sim.simx_opmode_blocking)
    
    # 获取关节当前角度（以弧度为单位）
    _, angle = sim.simxGetJointPosition(clientID, jointHandle, sim.simx_opmode_blocking)
    
    # 将弧度转换为角度
    angle_deg = angle * 180 / math.pi
    
    return angle_deg


def rotate_joint(clientID, joint_name, target_angle_deg):
    # 获取关节句柄
    _, jointHandle = sim.simxGetObjectHandle(clientID, joint_name, sim.simx_opmode_blocking)
    if jointHandle == -1:
        raise Exception(f"Failed to get handle for {joint_name}")

    # 将角度转换为弧度
    target_angle_rad = target_angle_deg *math.pi / 180
    
    # 设置关节目标位置
    res = sim.simxSetJointPosition(clientID, jointHandle, target_angle_rad, sim.simx_opmode_blocking)
    if res != sim.simx_return_ok:
        raise Exception(f"Failed to set target position for {joint_name}")

    # 等待关节到达目标位置
    # stime.sleep(abs(target_angle_deg) / target_velocity)

def rotate_joint_speed(clientID, joint_name, target_angle_deg,speed):
    _, jointHandle = sim.simxGetObjectHandle(clientID, joint_name, sim.simx_opmode_blocking)
    if jointHandle == -1:
        raise Exception(f"Failed to get handle for {joint_name}")
    
    current_angle=get_joint_angle(clientID, joint_name)
    
    if current_angle<target_angle_deg:
        while current_angle<target_angle_deg:
            rotate_joint(clientID,joint_name,current_angle+speed)
            current_angle+=speed
    
    elif current_angle>target_angle_deg:
        while current_angle>target_angle_deg:
            rotate_joint(clientID,joint_name,current_angle-speed)
            current_angle-=speed
    
    
def main():
    clientID = connect_to_coppeliasim()
    try:
        rotate_joint_speed(clientID, 'joint1', -30, 2)  
        ang = int(get_joint_angle(clientID, 'joint1'))
        print(ang)
        rotate_joint_speed(clientID, 'joint1', 0, 3)
    finally:
        sim.simxFinish(clientID)  # 关闭连接

if __name__ == '__main__':
    main()
